/**
* @file adis16460.h in ADIS16460_Reader
* @author rebeater
* @comment SPI driver for ADIS16460
* Create on 2021/1/29 下午8:06
* @version 1.0
**/

#include "main.h"
#include <adis16460.h>
#include <stdio.h>
#include <rtcommon.h>

#include "FreeRTOS.h"
#include "cmsis_os.h"




#define FLASH_CNT 0x00
#define DIAG_STAT 0x02

/* gyro and acce */
#define X_GYRO_LOW 0x04
#define X_GYRO_OUT 0x06
#define Y_GYRO_LOW 0x08
#define Y_GYRO_OUT 0x0A
#define Z_GYRO_LOW 0x0C
#define Z_GYRO_OUT 0x0E

#define X_ACCE_LOW 0x10
#define X_ACCE_OUT 0x12
#define Y_ACCE_LOW 0x14
#define Y_ACCE_OUT 0x16
#define Z_ACCE_LOW 0x18
#define Z_ACCE_OUT 0x1A

#define X_GYRO_OFF 0x40
#define Y_GYRO_OFF 0x42
#define Z_GYRO_OFF 0x44
#define X_ACCE_OFF 0x46
#define Y_ACCE_OFF 0x48
#define Z_ACCE_OFF 0x4A


#define MSC_CTRL 0x32
#define SYNC_SCAL 0x34
#define DEC_RATE 0x36
#define FLTR_CTRL 0x38
#define GLOB_CMD 0x3E

#define ADIS16460_WHO_AM_I 0x56


/*GLOB*/
#define GLOB_RESET (1u<<7u)
//#define GLOB_RESET (1<<7)
/*MSC_CTRL*/
#define MSC_G_COMPASATE (1u<<7u)
#define MSC_DR_UP (1)
#define MSC_DR_DOWN (0)

#define SPI_CS_DOWN()  HAL_GPIO_WritePin(SPI5_CS_GPIO_Port,SPI5_CS_Pin,GPIO_PIN_RESET)
#define SPI_CS_UP()  HAL_GPIO_WritePin(SPI5_CS_GPIO_Port,SPI5_CS_Pin,GPIO_PIN_SET)
#define HSPI hspi5
//const double PI = 3.1415926535897932384626;
#define PI 3.1415926535897932384626
/*scale factor*/
const double adis16460_ka_g = 0.25e-3;
const double adis16460_kg = 0.005 * PI / 180.0;

void SpiWriteReg(uint8_t addr, uint8_t cmd) {
    uint8_t dat[2];
    SPI_CS_DOWN();
    dat[1] = addr | 0x80u;
    dat[0] = cmd;
//    uint16_t dat = 0xBE01;
HAL_SPI_Transmit(&HSPI, (uint8_t *) dat, 1, 0xFFFF);
    SPI_CS_UP();
}

/*一个寄存器两个字节*/
uint16_t SpiReadReg(uint8_t addr) {
    uint8_t dat[4] = {0};
    SPI_CS_DOWN();
    dat[1] = addr;
    HAL_SPI_TransmitReceive(&HSPI, dat, dat, 2, 0xfff);
    HAL_SPI_Receive(&HSPI, dat, 2, 0xfff);
//    HAL_SPI_TransmitReceive(&HSPI,dat+1, dat,  1,0xfff);
    SPI_CS_UP();
    return *(uint16_t *) (&dat[0]);
}

int check_sum(const uint8_t data[20]) {
    uint16_t sum = 0;
    for (int i = 0; i < 18; i++) {
        sum += data[i];
    }
    return sum == *(uint16_t *) &data[18];
}

/**
 * burst reading
 * @param imu
 * @return
 */
int adis16460_burst_read(ImuRawAdi *imu) {
    SPI_CS_DOWN();
    uint16_t cmd = 0x3E00;
    HAL_SPI_Transmit(&HSPI, (uint8_t *) &cmd, 1, 0xfff);
    HAL_SPI_Receive(&HSPI, (uint8_t *) imu, 10, 0xfff);
    SPI_CS_UP();
    /*checksum*/
    return check_sum((uint8_t *) imu);
}


void adis16460_init() {
    uint16_t reg;
    ImuRawAdi adi;
    const uint16_t ADIS16460_ID = 0x404C;

    do {
        reg = SpiReadReg(ADIS16460_WHO_AM_I);
        delay(1000);
    } while (reg != ADIS16460_ID);

    /*上电自检*/
  SpiWriteReg(GLOB_CMD, 0x04);
    delay(100);
    adis16460_burst_read(&adi);
    if(adi.stat!= 0){
        trace(Info,"ADIS1640 Self-Test FAILED, Stat = %d\n",adi.stat);
        Error_Handler();
    }
    /*disable sync, set dr to 1 when data ready*/
  SpiWriteReg(MSC_CTRL, 0xC1);
    reg = SpiReadReg(MSC_CTRL);
    printf("MSC_CTRL = %X\n", reg);
    /*sample rate = 1024/(DEC_RATE +1)*/
  SpiWriteReg(DEC_RATE, 0x0f);/*2048/10 = 204.8Hz*/
  SpiWriteReg(0x37, 0x00);/*2048/10 = 204.8Hz*/
    reg = SpiReadReg(DEC_RATE);
    printf("DEC_RATE = %X\n", reg);
//    spi_write_reg(FLTR_CTRL, 4);/*filter*/
}

void convert_adi_to_double(ImuRawAdi *adi, ImuData *imu) {
    imu->gyro[0] = adis16460_kg * adi->gyro[0];
    imu->gyro[1] = adis16460_kg * adi->gyro[1];
    imu->gyro[2] = adis16460_kg * adi->gyro[2];

    imu->acce[0] = adis16460_ka_g * adi->acce[0];
    imu->acce[1] = adis16460_ka_g * adi->acce[1];
    imu->acce[2] = adis16460_ka_g * adi->acce[2];
}